﻿namespace JelloPhysics
{
    using Microsoft.Xna.Framework;
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.InteropServices;

    public class Body : IEnumerable<PointMass>, IEnumerable
    {
        protected AABB mAABB;
        protected ClosedShape mBaseShape;
        internal Bitmask mBitMaskX;
        internal Bitmask mBitMaskY;
        protected float mDerivedAngle;
        protected float mDerivedOmega;
        protected Vector2 mDerivedPos;
        protected Vector2 mDerivedVel;
        protected Vector2[] mGlobalShape;
        protected bool mIsStatic;
        protected bool mKinematic;
        protected float mLastAngle;
        protected int mMaterial;
        protected object mObjectTag;
        protected List<PointMass> mPointMasses;
        protected Vector2 mScale;
        protected float mVelDamping;

        public Body(World w)
        {
            this.mVelDamping = 0.999f;
            this.mBitMaskX = new Bitmask();
            this.mBitMaskY = new Bitmask();
            this.mAABB = new AABB();
            this.mBaseShape = null;
            this.mGlobalShape = null;
            this.mPointMasses = new List<PointMass>();
            this.mScale = Vector2.One;
            this.mIsStatic = false;
            this.mKinematic = false;
            this.mMaterial = 0;
            w.addBody(this);
        }

        public Body(World w, ClosedShape shape, List<float> pointMasses, Vector2 position, float angleInRadians, Vector2 scale, bool kinematic)
        {
            this.mVelDamping = 0.999f;
            this.mBitMaskX = new Bitmask();
            this.mBitMaskY = new Bitmask();
            this.mAABB = new AABB();
            this.mDerivedPos = position;
            this.mDerivedAngle = angleInRadians;
            this.mLastAngle = this.mDerivedAngle;
            this.mScale = scale;
            this.mMaterial = 0;
            this.mIsStatic = false;
            this.mKinematic = kinematic;
            this.mPointMasses = new List<PointMass>();
            this.setShape(shape);
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                this.mPointMasses[i].Mass = pointMasses[i];
            }
            this.updateAABB(0f, true);
            w.addBody(this);
        }

        public Body(World w, ClosedShape shape, float massPerPoint, Vector2 position, float angleInRadians, Vector2 scale, bool kinematic)
        {
            this.mVelDamping = 0.999f;
            this.mBitMaskX = new Bitmask();
            this.mBitMaskY = new Bitmask();
            this.mAABB = new AABB();
            this.mDerivedPos = position;
            this.mDerivedAngle = angleInRadians;
            this.mLastAngle = this.mDerivedAngle;
            this.mScale = scale;
            this.mMaterial = 0;
            this.mIsStatic = float.IsPositiveInfinity(massPerPoint);
            this.mKinematic = kinematic;
            this.mPointMasses = new List<PointMass>();
            this.setShape(shape);
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                this.mPointMasses[i].Mass = massPerPoint;
            }
            this.updateAABB(0f, true);
            w.addBody(this);
        }

        public virtual void accumulateExternalForces()
        {
        }

        public virtual void accumulateInternalForces()
        {
        }

        public void addGlobalForce(ref Vector2 pt, ref Vector2 force)
        {
            Vector2 vec = this.mDerivedPos - pt;
            float z = Vector3.Cross(VectorTools.vec3FromVec2(vec), VectorTools.vec3FromVec2((Vector2) force)).Z;
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                Vector2 vector2 = this.mPointMasses[i].Position - this.mDerivedPos;
                Vector2 vector3 = VectorTools.rotateVector(vector2, -1.570796f);
                PointMass local1 = this.mPointMasses[i];
                local1.Force += (Vector2) (vector3 * z);
                PointMass local2 = this.mPointMasses[i];
                local2.Force += force;
            }
        }

        public bool contains(ref Vector2 pt)
        {
            Vector2 vector = new Vector2();
            vector.X = this.mAABB.Max.X + 0.1f;
            vector.Y = pt.Y;
            bool flag = false;
            Vector2 position = this.mPointMasses[0].Position;
            Vector2 vector3 = new Vector2();
            int count = this.mPointMasses.Count;
            for (int i = 0; i < count; i++)
            {
                if (i < (count - 1))
                {
                    vector3 = this.mPointMasses[i + 1].Position;
                }
                else
                {
                    vector3 = this.mPointMasses[0].Position;
                }
                if (((position.Y <= pt.Y) && (vector3.Y > pt.Y)) || ((position.Y > pt.Y) && (vector3.Y <= pt.Y)))
                {
                    float num3 = (vector3.X - position.X) / (vector3.Y - position.Y);
                    float num4 = position.X + ((pt.Y - position.Y) * num3);
                    if ((num4 >= pt.X) && (num4 <= vector.X))
                    {
                        flag = !flag;
                    }
                }
                position = vector3;
            }
            return flag;
        }

        internal void dampenVelocity()
        {
            if (!this.mIsStatic)
            {
                for (int i = 0; i < this.mPointMasses.Count; i++)
                {
                    this.mPointMasses[i].Velocity.X *= this.mVelDamping;
                    this.mPointMasses[i].Velocity.Y *= this.mVelDamping;
                }
            }
        }

        public void derivePositionAndAngle(float elaspsed)
        {
            if (!this.mIsStatic && !this.mKinematic)
            {
                Vector2 vector = new Vector2();
                vector.X = 0f;
                vector.Y = 0f;
                Vector2 vector2 = new Vector2();
                vector2.X = 0f;
                vector2.Y = 0f;
                for (int i = 0; i < this.mPointMasses.Count; i++)
                {
                    vector.X += this.mPointMasses[i].Position.X;
                    vector.Y += this.mPointMasses[i].Position.Y;
                    vector2.X += this.mPointMasses[i].Velocity.X;
                    vector2.Y += this.mPointMasses[i].Velocity.Y;
                }
                vector.X /= (float) this.mPointMasses.Count;
                vector.Y /= (float) this.mPointMasses.Count;
                vector2.X /= (float) this.mPointMasses.Count;
                vector2.Y /= (float) this.mPointMasses.Count;
                this.mDerivedPos = vector;
                this.mDerivedVel = vector2;
                float num2 = 0f;
                int num3 = 1;
                float num4 = 0f;
                for (int j = 0; j < this.mPointMasses.Count; j++)
                {
                    float num6;
                    Vector2 vector3 = new Vector2();
                    vector3.X = this.mBaseShape.Vertices[j].X;
                    vector3.Y = this.mBaseShape.Vertices[j].Y;
                    Vector2.Normalize(ref vector3, out vector3);
                    Vector2 vector4 = new Vector2();
                    vector4.X = this.mPointMasses[j].Position.X - this.mDerivedPos.X;
                    vector4.Y = this.mPointMasses[j].Position.Y - this.mDerivedPos.Y;
                    Vector2.Normalize(ref vector4, out vector4);
                    Vector2.Dot(ref vector3, ref vector4, out num6);
                    if (num6 > 1f)
                    {
                        num6 = 1f;
                    }
                    if (num6 < -1f)
                    {
                        num6 = -1f;
                    }
                    float num7 = (float) Math.Acos((double) num6);
                    if (!VectorTools.isCCW(ref vector3, ref vector4))
                    {
                        num7 = -num7;
                    }
                    if (j == 0)
                    {
                        num3 = (num7 >= 0f) ? 1 : -1;
                        num4 = num7;
                    }
                    else
                    {
                        float num8 = num7 - num4;
                        int num9 = (num7 >= 0f) ? 1 : -1;
                        if ((Math.Abs(num8) > 3.1415926535897931) && (num9 != num3))
                        {
                            num7 = (num9 == -1) ? (3.141593f + (3.141593f + num7)) : ((3.141593f - num7) - 3.141593f);
                        }
                    }
                    num2 += num7;
                }
                num2 /= (float) this.mPointMasses.Count;
                this.mDerivedAngle = num2;
                float num10 = this.mDerivedAngle - this.mLastAngle;
                if (Math.Abs(num10) >= 3.1415926535897931)
                {
                    if (num10 < 0f)
                    {
                        num10 += 6.283185f;
                    }
                    else
                    {
                        num10 -= 6.283185f;
                    }
                }
                this.mDerivedOmega = num10 / elaspsed;
                this.mLastAngle = this.mDerivedAngle;
            }
        }

        public AABB getAABB()
        {
            return this.mAABB;
        }

        public float getClosestPoint(Vector2 pt, out Vector2 hitPt, out Vector2 normal, out int pointA, out int pointB, out float edgeD)
        {
            hitPt = Vector2.Zero;
            pointA = -1;
            pointB = -1;
            edgeD = 0f;
            normal = Vector2.Zero;
            float num = 1000f;
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                Vector2 vector;
                Vector2 vector2;
                float num3;
                float num4 = this.getClosestPointOnEdge(pt, i, out vector, out vector2, out num3);
                if (num4 < num)
                {
                    num = num4;
                    pointA = i;
                    if (i < (this.mPointMasses.Count - 1))
                    {
                        pointB = i + 1;
                    }
                    else
                    {
                        pointB = 0;
                    }
                    edgeD = num3;
                    normal = vector2;
                    hitPt = vector;
                }
            }
            return num;
        }

        public int getClosestPointMass(Vector2 pos, out float dist)
        {
            float num = 100000f;
            int num2 = -1;
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                float num4 = (pos - this.mPointMasses[i].Position).LengthSquared();
                if (num4 < num)
                {
                    num = num4;
                    num2 = i;
                }
            }
            dist = (float) Math.Sqrt((double) num);
            return num2;
        }

        public float getClosestPointOnEdge(Vector2 pt, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD)
        {
            float num3;
            hitPt = new Vector2();
            hitPt.X = 0f;
            hitPt.Y = 0f;
            normal = new Vector2();
            normal.X = 0f;
            normal.Y = 0f;
            edgeD = 0f;
            float result = 0f;
            Vector2 position = this.mPointMasses[edgeNum].Position;
            Vector2 vector2 = new Vector2();
            if (edgeNum < (this.mPointMasses.Count - 1))
            {
                vector2 = this.mPointMasses[edgeNum + 1].Position;
            }
            else
            {
                vector2 = this.mPointMasses[0].Position;
            }
            Vector2 vector3 = new Vector2();
            vector3.X = pt.X - position.X;
            vector3.Y = pt.Y - position.Y;
            Vector2 vIn = new Vector2();
            vIn.X = vector2.X - position.X;
            vIn.Y = vector2.Y - position.Y;
            float num2 = (float) Math.Sqrt((double) ((vIn.X * vIn.X) + (vIn.Y * vIn.Y)));
            if (num2 > 1E-05f)
            {
                vIn.X /= num2;
                vIn.Y /= num2;
            }
            Vector2 vOut = new Vector2();
            VectorTools.getPerpendicular(ref vIn, ref vOut);
            Vector2.Dot(ref vector3, ref vIn, out num3);
            if (num3 <= 0f)
            {
                Vector2.Distance(ref pt, ref position, out result);
                hitPt = position;
                edgeD = 0f;
                normal = vOut;
                return result;
            }
            if (num3 >= num2)
            {
                Vector2.Distance(ref pt, ref vector2, out result);
                hitPt = vector2;
                edgeD = 1f;
                normal = vOut;
                return result;
            }
            Vector3 vector6 = new Vector3();
            vector6.X = vector3.X;
            vector6.Y = vector3.Y;
            Vector3 vector7 = new Vector3();
            vector7.X = vIn.X;
            vector7.Y = vIn.Y;
            Vector3.Cross(ref vector6, ref vector7, out vector7);
            result = Math.Abs(vector7.Z);
            hitPt.X = position.X + (vIn.X * num3);
            hitPt.Y = position.Y + (vIn.Y * num3);
            edgeD = num3 / num2;
            normal = vOut;
            return result;
        }

        public float getClosestPointOnEdgeSquared(Vector2 pt, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD)
        {
            float num3;
            hitPt = new Vector2();
            hitPt.X = 0f;
            hitPt.Y = 0f;
            normal = new Vector2();
            normal.X = 0f;
            normal.Y = 0f;
            edgeD = 0f;
            float result = 0f;
            Vector2 position = this.mPointMasses[edgeNum].Position;
            Vector2 vector2 = new Vector2();
            if (edgeNum < (this.mPointMasses.Count - 1))
            {
                vector2 = this.mPointMasses[edgeNum + 1].Position;
            }
            else
            {
                vector2 = this.mPointMasses[0].Position;
            }
            Vector2 vector3 = new Vector2();
            vector3.X = pt.X - position.X;
            vector3.Y = pt.Y - position.Y;
            Vector2 vIn = new Vector2();
            vIn.X = vector2.X - position.X;
            vIn.Y = vector2.Y - position.Y;
            float num2 = (float) Math.Sqrt((double) ((vIn.X * vIn.X) + (vIn.Y * vIn.Y)));
            if (num2 > 1E-05f)
            {
                vIn.X /= num2;
                vIn.Y /= num2;
            }
            Vector2 vOut = new Vector2();
            VectorTools.getPerpendicular(ref vIn, ref vOut);
            Vector2.Dot(ref vector3, ref vIn, out num3);
            if (num3 <= 0f)
            {
                Vector2.DistanceSquared(ref pt, ref position, out result);
                hitPt = position;
                edgeD = 0f;
                normal = vOut;
                return result;
            }
            if (num3 >= num2)
            {
                Vector2.DistanceSquared(ref pt, ref vector2, out result);
                hitPt = vector2;
                edgeD = 1f;
                normal = vOut;
                return result;
            }
            Vector3 vector6 = new Vector3();
            vector6.X = vector3.X;
            vector6.Y = vector3.Y;
            Vector3 vector7 = new Vector3();
            vector7.X = vIn.X;
            vector7.Y = vIn.Y;
            Vector3.Cross(ref vector6, ref vector7, out vector7);
            result = Math.Abs((float) (vector7.Z * vector7.Z));
            hitPt.X = position.X + (vIn.X * num3);
            hitPt.Y = position.Y + (vIn.Y * num3);
            edgeD = num3 / num2;
            normal = vOut;
            return result;
        }

        public IEnumerator<PointMass> GetEnumerator()
        {
            return this.mPointMasses.Cast<PointMass>().GetEnumerator();
        }

        public PointMass getPointMass(int index)
        {
            return this.mPointMasses[index];
        }

        internal void integrate(float elapsed)
        {
            if (!this.mIsStatic)
            {
                for (int i = 0; i < this.mPointMasses.Count; i++)
                {
                    this.mPointMasses[i].integrateForce(elapsed);
                }
            }
        }

        public virtual void setKinematicAngle(float angleInRadians)
        {
            this.mDerivedAngle = angleInRadians;
        }

        public virtual void setKinematicPosition(ref Vector2 pos)
        {
            this.mDerivedPos = pos;
        }

        public virtual void setKinematicScale(ref Vector2 scale)
        {
            this.mScale = scale;
        }

        public void setMassAll(float mass)
        {
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                this.mPointMasses[i].Mass = mass;
            }
            if (float.IsPositiveInfinity(mass))
            {
                this.mIsStatic = true;
            }
        }

        public void setMassFromList(List<float> masses)
        {
            if (masses.Count == this.mPointMasses.Count)
            {
                for (int i = 0; i < this.mPointMasses.Count; i++)
                {
                    this.mPointMasses[i].Mass = masses[i];
                }
            }
        }

        public void setMassIndividual(int index, float mass)
        {
            if ((index >= 0) && (index < this.mPointMasses.Count))
            {
                this.mPointMasses[index].Mass = mass;
            }
        }

        public virtual void setPositionAngle(Vector2 pos, float angleInRadians, Vector2 scale)
        {
            this.mBaseShape.transformVertices(ref pos, angleInRadians, ref scale, ref this.mGlobalShape);
            for (int i = 0; i < this.mPointMasses.Count; i++)
            {
                this.mPointMasses[i].Position = this.mGlobalShape[i];
            }
            this.mDerivedPos = pos;
            this.mDerivedAngle = angleInRadians;
        }

        public void setShape(ClosedShape shape)
        {
            this.mBaseShape = shape;
            if (this.mBaseShape.Vertices.Count != this.mPointMasses.Count)
            {
                this.mPointMasses.Clear();
                this.mGlobalShape = new Vector2[this.mBaseShape.Vertices.Count];
                this.mBaseShape.transformVertices(ref this.mDerivedPos, this.mDerivedAngle, ref this.mScale, ref this.mGlobalShape);
                for (int i = 0; i < this.mBaseShape.Vertices.Count; i++)
                {
                    this.mPointMasses.Add(new PointMass(0f, this.mGlobalShape[i]));
                }
            }
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }

        public void updateAABB(float elapsed, bool forceUpdate)
        {
            if (!this.IsStatic || forceUpdate)
            {
                this.mAABB.clear();
                for (int i = 0; i < this.mPointMasses.Count; i++)
                {
                    Vector2 position = this.mPointMasses[i].Position;
                    this.mAABB.expandToInclude(ref position);
                    if (!this.IsStatic)
                    {
                        position.X += this.mPointMasses[i].Velocity.X * elapsed;
                        position.Y += this.mPointMasses[i].Velocity.Y * elapsed;
                        this.mAABB.expandToInclude(ref position);
                    }
                }
            }
        }

        public float DerivedAngle
        {
            get
            {
                return this.mDerivedAngle;
            }
        }

        public float DerivedOmega
        {
            get
            {
                return this.mDerivedOmega;
            }
        }

        public Vector2 DerivedPosition
        {
            get
            {
                return this.mDerivedPos;
            }
        }

        public Vector2 DerivedVelocity
        {
            get
            {
                return this.mDerivedVel;
            }
        }

        public bool IsKinematic
        {
            get
            {
                return this.mKinematic;
            }
            set
            {
                this.mKinematic = value;
            }
        }

        public bool IsStatic
        {
            get
            {
                return this.mIsStatic;
            }
            set
            {
                this.mIsStatic = value;
            }
        }

        public int Material
        {
            get
            {
                return this.mMaterial;
            }
            set
            {
                this.mMaterial = value;
            }
        }

        public object ObjectTag
        {
            get
            {
                return this.mObjectTag;
            }
            set
            {
                this.mObjectTag = value;
            }
        }

        public int PointMassCount
        {
            get
            {
                return this.mPointMasses.Count;
            }
        }

        public float VelocityDamping
        {
            get
            {
                return this.mVelDamping;
            }
            set
            {
                this.mVelDamping = value;
            }
        }
    }
}

